﻿module Physics.Chapter2.RigidBody

open Casanova
open Casanova.Input
open Casanova.Core
open Casanova.Utilities
open Casanova.Math
open Casanova.Game
open Casanova.Drawing
open Casanova.StandardLibrary
open Casanova.StandardLibrary.Core
open Microsoft.Xna.Framework.Graphics

let mutable private max_index = 0

type [<CasanovaEntity>] RigidBody = {
  Model               : Casanova.Drawing.Model

  Size                : Vector3<m>
  MassInv             : float32<kg^-1>
  IInv                : Matrix

  Position            : Rule<Vector3<m>>
  LinearMomentum      : Rule<Vector3<kg * m / s>>
  Rotation            : Rule<Quaternion>
  AngularMomentum     : Rule<Vector3<kg * m^2 / s>>

  F                   : Rule<Vector3<kg * m / s^2>>
  T                   : Rule<Vector3<kg * m^2 / s^2>>

  TickCount           : Rule<int>

  mutable Index               : int
} with
  static member ModelPosition'(self : RigidBody) = !self.Position
  static member ModelRotation'(self : RigidBody) = !self.Rotation

  member body.AddF(F : Vector3<N>) =
    body.F := !body.F + F
  member body.AddT(T : Vector3<N * m>) =
    body.T := !body.T + T

  member self.Velocity = !self.LinearMomentum * self.MassInv
  static member Position'(self : RigidBody, dt : float32<s>) =
    !self.Position + self.Velocity * dt
  static member LinearMomentum'(self : RigidBody, dt : float32<s>) =
    !self.LinearMomentum + !self.F * dt
  static member F'(self : RigidBody, dt : float32<s>) =
    Vector3.Zero

  member self.R = Matrix.CreateFromQuaternion(!self.Rotation)
  member self.RInverse = Matrix.Transpose(self.R)
  member self.InverseI = 
    let R = Matrix.CreateFromQuaternion(!self.Rotation)
    R * self.IInv * Matrix.Transpose(R)
  member self.AngularVelocity = Microsoft.Xna.Framework.Vector3.Transform((!self.AngularMomentum).ToXNA, self.InverseI)
  member self.RotationVelocity = 
    let w = Microsoft.Xna.Framework.Quaternion(self.AngularVelocity, 0.0f)
    (w * !self.Rotation) * 0.5f
  static member Rotation'(self : RigidBody, dt : float32<s>) =
    let r' = !self.Rotation + self.RotationVelocity * (float32 dt)
    if !self.TickCount % 10 = 0 then
      Microsoft.Xna.Framework.Quaternion.Normalize(r')
    else
      r'
  static member AngularMomentum'(self : RigidBody, dt : float32<s>) =
    !self.AngularMomentum + !self.T * dt
  static member T'(self : RigidBody, dt : float32<s>) =
    Vector3.Zero

  member body.Immovable = body.MassInv = 0.0f<kg^-1>

  member body.FromLocalSpace(p:Vector3<m>) = body.R * p + !body.Position
  member body.FromLocalSpaceNormal(n:Vector3<'a>) = body.R * n
  member body.ToLocalSpace(p:Vector3<m>) = 
    Microsoft.Xna.Framework.Matrix.Transpose(body.R) * (p - !body.Position)
  member body.ToLocalSpaceNormal(p:Vector3<'a>) = 
    Microsoft.Xna.Framework.Matrix.Transpose(body.R) * p

  static member CreateCube(model_layer, position, size : Vector3<_>, mass_inv, friction_coefficient, path) =
    let cube_model  = Model.Create(model_layer, position, size * 0.5f, Microsoft.Xna.Framework.Quaternion.Identity, path)
    let mutable cube_IInv = Matrix.Identity
    cube_IInv.M11 <- float32(12.0f * mass_inv / (size.Y * size.Y + size.Z * size.Z))
    cube_IInv.M22 <- float32(12.0f * mass_inv / (size.X * size.X + size.Z * size.Z))
    cube_IInv.M33 <- float32(12.0f * mass_inv / (size.X * size.X + size.Y * size.Y))

    do max_index <- max_index + 1

    {
      Model               = cube_model

      Size                = size
      MassInv             = mass_inv
      IInv                = cube_IInv

      Position            = Rule.Create(position)
      LinearMomentum      = Rule.Create(Vector3.Zero)
      Rotation            = Rule.Create(Quaternion.Identity)
      AngularMomentum     = Rule.Create(Vector3.Zero)

      F                   = Rule.Create(Vector3.Zero)
      T                   = Rule.Create(Vector3.Zero)

      TickCount           = Rule.Create(0)
  
      Index               = max_index - 1
    }

  member self.ApplyExternalForce(r : Vector3<m>, f_ext : Vector3<N>) =
    let r     = self.ToLocalSpace(r).ToXNA
    let f_ext = f_ext.ToXNA

    let F = r * Microsoft.Xna.Framework.Vector3.Dot(f_ext, r)
    let T = Microsoft.Xna.Framework.Vector3.Cross(r, f_ext)

    self.F := !self.F + Vector3<_>(F)
    self.T := !self.T + Vector3<_>(T)
